#!/usr/bin/env python3
# coding:utf-8
import sys
import getopt
import threading
import rclpy
from rclpy.node import Node
from std_msgs.msg import Char, String
from nobloack_key_input import Noblock_terminal

Default_Topic_Name = "cmd"


class MinimalPublisher(Node):
    def __init__(self, topic_name):
        super().__init__(f"publish_{topic_name}")
        self.pub_cmd = self.create_publisher(String, f"/{topic_name}", 1)
        self.thread_init()

    def thread_init(self):
        t1 = threading.Thread(target=self.control_cmd, daemon=True)
        t1.start()

    def control_cmd(self):
        noblock_term = Noblock_terminal()
        msg = String()
        while True:
            key = noblock_term.noblock_get_cmd()
            msg.data = key
            self.pub_cmd.publish(msg)
            if key == "q":
                print("exit 退出")
                break

        noblock_term.stop_no_block()


def parse_args():
    global Default_Topic_Name
    topic_name = Default_Topic_Name
    try:
        opts, args = getopt.getopt(sys.argv[1:], "n:", ["name="])
    except getopt.GetoptError as err:
        print(err)
        sys.exit(2)
    for o, a in opts:
        if o in ("-n", "--name"):
            topic_name = a
        else:
            assert False, "unhandled option"
    return topic_name


def main(args=None):
    topic_name = parse_args()
    rclpy.init(args=args)
    ros_node = MinimalPublisher(topic_name)
    rclpy.spin(ros_node)
    ros_node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
